#include "my_utils.h"

void TestUtils::add_count()
{
    this->count++;
}

int TestUtils::get_count()
{
    return this->count * 10;
}

// ===========================================
//                  S型速度规划
// ===========================================

/**
 * @brief  len[start, end]数组相加
 * @param  start: 
 * @param  end: 
 * @return double: 
 */
double SpeedUtils::len_sum(int start, int end)
{
    double sum = 0;
    for (int i = start; i <= end; i++)
    {
        sum = sum + this->len[i];
    }
    return sum;
}

void SpeedUtils::setGoal(double overall)
{
    this->overall = overall;
    if (overall < this->choice[0])
    {
        // 未到达规定的amax
        this->time[1] = pow((overall / this->jerk_), 1.0 / 3);
        this->time[2] = 0;
        this->time[3] = this->time[1];
        this->time[4] = 0;
        this->time[5] = this->time[1];
        this->time[6] = 0;
        this->time[7] = this->time[1];

        this->cur_acc_max = this->jerk_ * this->time[1];
        this->len[1] = overall / 6;
        this->len[2] = 0;
        this->len[3] = this->cur_acc_max * pow(this->time[3], 2) - this->jerk_ * pow(this->time[3], 3) / 6;
        this->len[4] = 0;
        this->len[5] = this->len[3];
        this->len[6] = 0;
        this->len[7] = this->len[1];
    }
    else if (overall < this->choice[1])
    {
        // 到达规定的amax,未到达规定的vmax
        this->cur_acc_max = this->acc_max_;
        this->time[1] = (double)this->cur_acc_max / this->jerk_;
        this->time[2] = sqrt((overall / this->cur_acc_max) + pow(this->time[1], 2) / 4) - 3 * this->time[1] / 2;
        this->time[3] = this->time[1];
        this->time[4] = 0;
        this->time[5] = this->time[1];
        this->time[6] = this->time[2];
        this->time[7] = this->time[3];

        this->len[1] = this->cur_acc_max * pow(this->time[1], 2) / 6;
        this->len[2] = this->cur_acc_max * (this->time[1] * this->time[2] + pow(this->time[2], 2)) / 2;
        this->len[3] = this->cur_acc_max * this->time[3] * (this->time[1] + this->time[2]) - this->jerk_ * pow(this->time[3], 3) / 6;
        this->len[4] = 0;
        this->len[5] = this->len[3];
        this->len[6] = this->len[2];
        this->len[7] = this->len[1];
    }
    else
    {
        // 到达规定的vmax
        this->cur_acc_max = this->acc_max_;
        this->time[1] = (double)this->cur_acc_max / this->jerk_;
        this->time[2] = (this->speed_max_ - this->cur_acc_max * this->time[1]) / this->cur_acc_max;
        this->time[3] = this->time[1];
        this->time[4] = (overall - this->cur_acc_max * (3 * this->time[1] * this->time[2] + pow(this->time[2], 2) + 2 * pow(this->time[1], 2))) / this->speed_max_;
        this->time[5] = this->time[1];
        this->time[6] = this->time[2];
        this->time[7] = this->time[3];

        this->len[1] = this->cur_acc_max * pow(this->time[1], 2) / 6;
        this->len[2] = this->cur_acc_max * (this->time[1] * this->time[2] + pow(this->time[2], 2)) / 2;
        this->len[3] = this->cur_acc_max * this->time[3] * (this->time[1] + this->time[2]) - this->jerk_ * pow(this->time[3], 3) / 6;
        this->len[4] = this->speed_max_ * this->time[4];
        this->len[5] = this->len[3];
        this->len[6] = this->len[2];
        this->len[7] = this->len[1];
    }
    
}

int SpeedUtils::getSpeed(int walked)
{
    if (walked < this->len[1])
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_acc = this->jerk_ * this->delta + this->cur_acc;
    }
    else if (walked < this->len_sum(1, 2))
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_acc = this->cur_acc_max;
    }
    else if (walked < this->len_sum(1, 3))
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_acc = this->cur_acc - this->jerk_ * this->delta;
    }
    else if (walked < this->len_sum(1, 4))
    {
        this->cur_speed = this->speed_max_;
        this->cur_acc = 0;
    }
    else if (walked < this->len_sum(1, 5))
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_acc = this->cur_acc - this->jerk_ * this->delta;
    }
    else if (walked < this->len_sum(1, 6))
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_acc = - this->cur_acc_max;
    }
    else
    {
        this->cur_speed = this->cur_speed + (int)this->cur_acc * this->delta;
        this->cur_acc = this->cur_acc + this->jerk_ * this->delta;
    }
    return this->cur_speed;
}